// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ColorDepthAlignment.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Cameras
{
    using System;
    using System.Collections.Generic;
    using System.Collections.ObjectModel;
    using System.Linq;
    using System.Reflection;
    using System.Runtime.CompilerServices;
    using System.Text;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Numerics;
    
    /// <summary>
    /// Base class for creating mappings a depth and a color image
    /// </summary>
    public class ColorDepthAlignment
    {
        /// <summary>
        /// The alignment class
        /// </summary>
        private object alignment;

        /// <summary>
        /// The alignment method
        /// </summary>
        private MethodInfo alignMethod;

        /// <summary>
        /// The color to depth method
        /// </summary>
        private MethodInfo colorOfDepthPixelMethod;

        /// <summary>
        /// The depth to color method
        /// </summary>
        private MethodInfo depthOfRgbPixelMethod;

        /// <summary>
        /// The color to 3D method
        /// </summary>
        private MethodInfo colorPixelTo3DMethod;

        /// <summary>
        /// The depth to 3D method
        /// </summary>
        private MethodInfo depthPixelTo3DMethod;

        /// <summary>
        /// The $LocationOfDepthPixelinColor$ method
        /// </summary>
        private MethodInfo locationOfDepthMethod;

        /// <summary>
        /// The $LocationOfColorPixelinDepth$ method
        /// </summary>
        private MethodInfo locationOfColorMethod;

        /// <summary>
        /// The 3D point to color method
        /// </summary>
        private MethodInfo point3DToColorMethod;

        /// <summary>
        /// The 3D point to depth method
        /// </summary>
        private MethodInfo point3DToDepthMethod;
        
        /// <summary>
        /// Initializes a new instance of the ColorDepthAlignment class
        /// </summary>
        public ColorDepthAlignment()
        {
        }

        /// <summary>
        /// Initializes a new instance of the ColorDepthAlignment class
        /// </summary>
        /// <param name="type">The type of color depth alignment</param>
        /// <param name="depth">The depth camera parameters</param>
        /// <param name="color">The color camera parameters</param>
        public ColorDepthAlignment(Type type, CameraParameters depth, CameraParameters color)
        {
            this.alignment = null;

            if (type != null)
            {
                ConstructorInfo methodInfo = type.GetConstructor(new Type[] { typeof(CameraParameters), typeof(CameraParameters) });

                if (methodInfo == null)
                {
                    throw new Exception("Type " + type + " does implement a constructor of interface " + type + "(CameraParameters, CameraParameters)");
                }

                this.alignment = methodInfo.Invoke(new object[] { depth, color });

                this.alignMethod = type.GetMethod("Align", new Type[] { typeof(ImageFrame<short>), typeof(ImageFrame<RgbPix>), typeof(CameraParameters), typeof(CameraParameters) });
                this.colorOfDepthPixelMethod = type.GetMethod("ColorOfDepthPixel", new Type[] { typeof(int), typeof(int) });
                this.depthOfRgbPixelMethod = type.GetMethod("DepthOfRgbPixel", new Type[] { typeof(int), typeof(int) });
                this.colorPixelTo3DMethod = type.GetMethod("ColorPixelTo3D", new Type[] { typeof(int), typeof(int) });
                this.depthPixelTo3DMethod = type.GetMethod("DepthPixelTo3D", new Type[] { typeof(int), typeof(int) });
                this.locationOfDepthMethod = type.GetMethod("LocationOfDepthPixelinColor", new Type[] { typeof(int), typeof(int) });
                this.locationOfColorMethod = type.GetMethod("LocationOfColorPixelinDepth", new Type[] { typeof(int), typeof(int) });
                this.point3DToColorMethod = type.GetMethod("Point3DToColor", new Type[] { typeof(Vector3) });
                this.point3DToDepthMethod = type.GetMethod("Point3DToDepth", new Type[] { typeof(Vector3) });
            }
        }

        /// <summary>
        /// Computes the mapping given a depth image
        /// </summary>
        /// <param name="depth">The depth image</param>
        /// <param name="color">the color image</param>
        /// <param name="depthParams">The depth camera parameters</param>
        /// <param name="colorParams">The color camera parameters</param>
        public virtual void Align(ImageFrame<short> depth, ImageFrame<RgbPix> color, CameraParameters depthParams, CameraParameters colorParams)
        {
            if (this.alignment != null && this.alignMethod != null)
            {
                this.alignMethod.Invoke(this.alignment, new object[] { depth, color, depthParams, colorParams });
            }
        }

        /// <summary>
        /// Inline function that returns a depth pixel mapped to a color pixel
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">the y value of the pixel</param>
        /// <returns>The color of the depth pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public virtual RgbPix ColorOfDepthPixel(int x, int y)
        {
            if (this.alignment != null && this.colorOfDepthPixelMethod != null)
            {
                return (RgbPix)this.colorOfDepthPixelMethod.Invoke(this.alignment, new object[] { x, y });
            }
            else
            {
                return new RgbPix(0, 0, 0);
            }
        }

        /// <summary>
        /// Inline function that returns the location of the depth pixel in the color image
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">the y value of the pixel</param>
        /// <returns>The location in the color image</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public virtual Point2D<int>? LocationOfDepthPixelinColor(int x, int y)
        {
            if (this.alignment != null && this.colorOfDepthPixelMethod != null)
            {
                return (Point2D<int>?)this.locationOfDepthMethod.Invoke(this.alignment, new object[] { x, y });
            }
            else
            {
                return null;
            }
        }

        /// <summary>
        /// Inline function that returns the location of the color pixel in the depth image
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">the y value of the pixel</param>
        /// <returns>The location in the depth image</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public virtual Point2D<int>? LocationOfColorPixelinDepth(int x, int y)
        {
            if (this.alignment != null && this.colorOfDepthPixelMethod != null)
            {
                Point2D<int>? point = (Point2D<int>?)this.locationOfColorMethod.Invoke(this.alignment, new object[] { x, y });
                return point;
            }
            else
            {
                return null;
            }
        }

        /// <summary>
        /// Inline function that returns a color pixel mapped to a depth pixel
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">the y value of the pixel</param>
        /// <returns>The depth at the color pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public virtual short DepthOfRgbPixel(int x, int y)
        {
            if (this.alignment != null && this.depthOfRgbPixelMethod != null)
            {
                return (short)this.depthOfRgbPixelMethod.Invoke(this.alignment, new object[] { x, y });
            }
            else
            {
                return 0;
            }
        }

        /// <summary>
        /// Inline function that returns the 3D point of a color pixel
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">The y value of the pixel</param>
        /// <returns>The 3D point projected onto the color image</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public virtual Vector3? ColorPixelTo3D(int x, int y)
        {
            if (this.alignment != null && this.colorPixelTo3DMethod != null)
            {
                return (Vector3?)this.colorPixelTo3DMethod.Invoke(this.alignment, new object[] { x, y });
            }
            else
            {
                return null;
            }
        }

        /// <summary>
        /// Inline function that returns the 3D point of a depth pixel
        /// </summary>
        /// <param name="x">The x value of the pixel</param>
        /// <param name="y">The y value of the pixel</param>
        /// <returns>The 3D point projected onto the depth pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public virtual Vector3? DepthPixelTo3D(int x, int y)
        {
            if (this.alignment != null && this.depthOfRgbPixelMethod != null)
            {
                return (Vector3?)this.depthPixelTo3DMethod.Invoke(this.alignment, new object[] { x, y });
            }
            else
            {
                return null;
            }
        }

        /// <summary>
        /// Inline function that returns the projection of a 3D point onto the color image
        /// </summary>
        /// <param name="point">The 3D point</param>
        /// <returns>The 3D point projected onto the depth pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public virtual Vector2? Point3DToColor(Vector3 point)
        {
            if (this.alignment != null && this.point3DToColorMethod != null)
            {
                return (Vector2?)this.point3DToColorMethod.Invoke(this.alignment, new object[] { point });
            }
            else
            {
                return null;
            }
        }

        /// <summary>
        /// Inline function that returns the projection of a 3D point onto the depth image
        /// </summary>
        /// <param name="point">The 3D point</param>
        /// <returns>The 3D point projected onto the depth pixel</returns>
        [MethodImplAttribute(MethodImplOptions.AggressiveInlining)]
        public virtual Vector2? Point3DToDepth(Vector3 point)
        {
            if (this.alignment != null && this.point3DToDepthMethod != null)
            {
                return (Vector2?)this.point3DToDepthMethod.Invoke(this.alignment, new object[] { point });
            }
            else
            {
                return null;
            }
        }
    }
}
